#include "Util.h"

bool CompareTo(const Particle* p1, const Particle* p2)
{
	return p1->pCost < p2->pCost;
}

vector<double> HypoToPosition(HypothesisX hypo)
{
	vector<double> position(vPOS_DOF);

	for (int i=0;i<3;i++) {
		position[vPOS_WRIST+i] = hypo.dofData[OFFSET_WRIST+i];
		position[vPOS_THUMB+i] = hypo.dofData[OFFSET_THUMB+i];
		position[vPOS_FINGER1+i] = hypo.dofData[OFFSET_FINGER1+i];
		position[vPOS_FINGER2+i] = hypo.dofData[OFFSET_FINGER2+i];
		position[vPOS_FINGER3+i] = hypo.dofData[OFFSET_FINGER3+i];
		position[vPOS_LITTLE+i] = hypo.dofData[OFFSET_LITTLE+i];
	}
	return position;
}

HypothesisX PositionXToHypo(vector<double> position)
{
	HypothesisX hypo;
	for (int i=0;i<3;i++) {
		hypo.dofData[OFFSET_WRIST+i] = position[vPOS_WRIST+i];
		hypo.dofData[OFFSET_THUMB+i] = position[vPOS_THUMB+i];
		hypo.dofData[OFFSET_FINGER1+i] = position[vPOS_FINGER1+i];
		hypo.dofData[OFFSET_FINGER2+i] = position[vPOS_FINGER2+i];
		hypo.dofData[OFFSET_FINGER3+i] = position[vPOS_FINGER3+i];
		hypo.dofData[OFFSET_LITTLE+i] = position[vPOS_LITTLE+i];
	}
	hypo.dofData[OFFSET_THUMB+3] = position[vPOS_THUMB+3];
	hypo.dofData[OFFSET_FINGER1+3] = (2/3)*position[vPOS_FINGER1+2];
	hypo.dofData[OFFSET_FINGER2+3] = (2/3)*position[vPOS_FINGER2+2];
	hypo.dofData[OFFSET_FINGER3+3] = (2/3)*position[vPOS_FINGER3+2];
	hypo.dofData[OFFSET_LITTLE+3] = (2/3)*position[vPOS_LITTLE+2];

	return	hypo;
}

void ShowHypo(HypothesisX hypo)
{
	//printf("wirst_Position: %.2f %.2f %.2f\n", hypo.wirst_Position[0],hypo.wirst_Position[1], hypo.wirst_Position[2]);
	printf("wirst_Rotation: %.2f %.2f %.2f\n", hypo.dofData[OFFSET_WRIST+0],hypo.dofData[OFFSET_WRIST+1], hypo.dofData[OFFSET_WRIST+2]);
	printf("thumb_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_THUMB+0],hypo.dofData[OFFSET_THUMB+1], hypo.dofData[OFFSET_THUMB+2], hypo.dofData[OFFSET_THUMB+3]);
	printf("finger1_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER1+0],hypo.dofData[OFFSET_FINGER1+1], hypo.dofData[OFFSET_FINGER1+2], hypo.dofData[OFFSET_FINGER1+3]);
	printf("finger2_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER2+0],hypo.dofData[OFFSET_FINGER2+1], hypo.dofData[OFFSET_FINGER2+2], hypo.dofData[OFFSET_FINGER2+3]);
	printf("finger3_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER3+0],hypo.dofData[OFFSET_FINGER3+1], hypo.dofData[OFFSET_FINGER3+2], hypo.dofData[OFFSET_FINGER3+3]);
	printf("little_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_LITTLE+0],hypo.dofData[OFFSET_LITTLE+1], hypo.dofData[OFFSET_LITTLE+2], hypo.dofData[OFFSET_LITTLE+3]);
	return;
}
